#ifndef COLLIDE_H
#define COLLIDE_H

#include <iostream>
#include <fstream>
#include <vector>

#define OZCOLLIDE_PCH
#include "ozcollide/ozcollide.h"

#include "GL/glut.h"
#include "utility.h"
#include "mesh.h"


inline void intersect_RayTriangle( Point3 & P0, Point3 & P1, Point3 V[3], Point & I )
{
    Vector3    u, v, n;             // triangle vectors
    Vector3   dir, w0, w;          // ray vectors
    float     r, a, b;             // params to calc ray-plane intersect

    // get triangle edge vectors and plane normal
    u = V[1] - V[0];
    v = V[2] - V[0];
    n = u ^ v;             // cross product

    dir = P1 - P0;             // ray direction vector
    w0 = P0 - V[0];
    a = (n*w0);
    b = (n*dir);
    r = a / b;

    I = P0 + dir * r;           // intersect point of ray and plane
	
    //return 1;                      // I is in T
}

class Collide
{
public:
	//Build AABBTree here
	Collide (Mesh * mesh): mesh(mesh)
	{
		for(unsigned i = 0; i < mesh->vertices.size(); i++)//reading vertices
		{
			//Add vertex positions
			Vertex * v = mesh->vertices[i]; 
			ozcollide::Vec3f tmpV(v->point.x,v->point.y,v->point.z);
			verts.add(tmpV);//adding each readed vertex to the list
		}
		for(unsigned i = 0; i < mesh->v_faces.size(); ++i)
		{

			//Add normals
			Vector3 normal = mesh->v_faces[i]->normal();
			normal.normalize(); 
			ozcollide::Vec3f tmpN(normal.x,normal.y,normal.z);
			normals.add(tmpN);//adding each readed normal to the list(one per face)

			//Add faces
			ozcollide::Polygon *p = new ozcollide::Polygon();//we must build each face before add it to the list
			unsigned *indx = new unsigned[3];
			ozcollide::Vec3f tmpNormal(normals[i]);
			mesh->v_faces[i]->Face2VertexIdx(indx);
			indices.add(indx[0]);
			indices.add(indx[1]);
			indices.add(indx[2]);
			p->setIndicesMemory(3,(int *)&indx[0]);//setting up indices
			p->setNormal(tmpNormal);//adding normals (previously readed)
			tris.add(* p);//adding the polygon to the polygon list
		}

		ozcollide::AABBTreePolyBuilder builder;
		BaseTree = builder.buildFromPolys(tris.mem(),//polygons
			tris.size(),//polygon count
			verts.mem(),//vertices
			verts.size());//vertices count
	}
	Face * segmentCollide (Point3 p1, Point3 p2)
	{
		//Do collision
		BaseTree->collideWithSegment(ozcollide::Vec3f(p1.x,p1.y,p1.z), 
			ozcollide::Vec3f(p2.x,p2.y,p2.z), segmentResult);
		//Get the faces 
		float distance = INFINITY ;
		Face * pick_face = NULL;
		for(int i = 0; i < segmentResult.polys_.size(); i++)
		{
			unsigned idx[3];
			for(int j = 0; j < 3; j++)
			{
				idx[j] = segmentResult.polys_[i]->getIndex(j) ;
			}
			Face * collideFace = mesh->faces[faceHashKey(idx[0], idx[1], idx[2])];
			//Filter the front face
			if (collideFace->normal() * (p1 - p2) >=0 )
			{
				Point3 v[3];
				Point3 insec ;
				collideFace->Face2VertexCoord(v);
				intersect_RayTriangle(p1, p2, v, insec);
				if ((insec - p1).length() < distance)
				{
					pick_face = collideFace ;
					distance = (insec - p1).length() ;
				}
			}
		}
		return pick_face ;
	}
	~ Collide ()
	{
		verts.clear();
		tris.clear();
	}
private:

	Mesh * mesh; 
	ozcollide::AABBTreePoly * BaseTree; 
	ozcollide::Vector<ozcollide::Vec3f> verts,normals;
	ozcollide::Vector<ozcollide::Polygon> tris;
	ozcollide::Vector<int> indices; 
	ozcollide::AABBTreePoly::SegmentColResult segmentResult ;

};


#endif